/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#ifndef PHYSICS_H_
#define PHYSICS_H_

#include "ode/ode.h"
#include "World.h"
#include "Shape.h"
#include "Entity.h"
#include "Joint.h"
#include "PhysicalSensor.h"
#include "TactileSensor.h"
#include <utility>
#include <map>
#include <set>

class Physics {
	friend class PhysicalSensor<TactileSensor>;
	private:
	static std::map<dJointFeedback*, std::pair<Shape*, Shape*> > feedbacks;
	static dWorldID dynWorld;
	static dSpaceID colSpace;
	static dJointGroupID contactGroup;
	static std::map<Entity*, dBodyID> bodies;
	static std::map<Joint*, dJointID> joints;
	static std::map<dBodyID, Vector3D> shifts;
	static std::map<Shape*, Vector3D> forces;
	static std::set<std::pair<dBodyID, dBodyID> > jointBodies;
	static int counter;
	static World *root;
	static void nearCallback (void *data, dGeomID o1, dGeomID o2);
	static dBodyID initEntity(Entity*, dVector3 globalPos, dMatrix3 globalRot);
	static dGeomID initShape(Entity*, dBodyID body, dSpaceID colSpace, dMass*, Shape*, dVector3 globalPos, dMatrix3 globalRot);
	static void initJoint(Joint* joint, dBodyID body1, dBodyID body2, dVector3 globalPos, dMatrix3 globalRot);
	static void updateEntity(Entity* entity, dVector3 globalPos, dMatrix3 globalRot);
	public:
	static void init(World*);
	static void step();
	static void cleanup();
	static void convertVector(Vector3D&, dVector3);
	static void convertQuaternion(Quaternion&, dQuaternion);
};

#endif /*PHYSICS_H_*/
